#include "outline/bezier.h"
//#include "gslmath/multifit.h"

namespace outline {

template <class R>
template <class T, class PI>
typename bezier<R>::real_type bezier<R>::best_fit_general(std::vector< point<T> > const & points, PI param_iter)
{
	uint n = points.size();
	//assert (n == parameters.size());
	assert (n >= 4);

	assert(*param_iter == 0);
	start() = points[0];
	//assert(parameters[n-1] == 1);
	finish() = points[n-1];

	point<accu_type> p0 = points[0];
	point<accu_type> pe = points[n-1];

	accu_type sum_r1_r1 = 0; ///< sum of square weights of 1st control point (divided on 9)
	accu_type sum_r1_r2 = 0; ///< ...
	accu_type sum_r2_r2 = 0;
	accu_type sum_r1_x = 0;  ///< sum of products of 1st control point weight and appropriate absciss value of target point (divided on 3)
	accu_type sum_r1_y = 0;
	accu_type sum_r2_x = 0;
	accu_type sum_r2_y = 0;
	accu_type sum_x_x = 0;
	accu_type sum_y_y = 0;

	++param_iter;
	for (uint i = 1; i < n-1; ++i, ++param_iter) {
		accu_type t = *param_iter;
		accu_type u = 1-t;
		accu_type r0 = u*u*u;
		accu_type r1 = t*u*u;
		accu_type r2 = u*t*t;
		accu_type r3 = t*t*t;
		point<accu_type> p = point<accu_type>(points[i]) - r0 * p0 - r3 * pe;
		sum_r1_r1 += r1 * r1;
		sum_r1_r2 += r1 * r2;
		sum_r2_r2 += r2 * r2;
		sum_r1_x += r1 * p.x;
		sum_r1_y += r1 * p.y;
		sum_r2_x += r2 * p.x;
		sum_r2_y += r2 * p.y;
		sum_x_x += p.x * p.x;
		sum_y_y += p.y * p.y;
	}

	accu_type inv_det = 1 / (sum_r1_r1 * sum_r2_r2 - sum_r1_r2 * sum_r1_r2);

	accu_type const one_third = accu_type(1) / 3;

	start_control().x = one_third * inv_det * (sum_r2_r2 * sum_r1_x - sum_r1_r2 * sum_r2_x);
	start_control().y = one_third * inv_det * (sum_r2_r2 * sum_r1_y - sum_r1_r2 * sum_r2_y);

	finish_control().x = one_third * inv_det * (-sum_r1_r2 * sum_r1_x + sum_r1_r1 * sum_r2_x);
	finish_control().y = one_third * inv_det * (-sum_r1_r2 * sum_r1_y + sum_r1_r1 * sum_r2_y);

	accu_type chisq_x = sum_x_x - 3 * (sum_r1_x * start_control().x + sum_r2_x * finish_control().x);
	accu_type chisq_y = sum_y_y - 3 * (sum_r1_y * start_control().y + sum_r2_y * finish_control().y);
	accu_type chisq = chisq_x + chisq_y;

	return (chisq >= 0) ? chisq : 0;
}

/*
This implementation uses GSL and rather slow due to memory allocation and so on

template <class R>
typename bezier<R>::real_type bezier<R>::best_fit(real_point_vector_type const & points, real_vector_type const & parameters)
{
	uint n = parameters.size();
	assert (n == points.size());
	assert (n >= 4);

	assert(parameters[0] == 0);
	start() = points[0];
	assert(parameters[n-1] == 1);
	finish() = points[n-1];

	multifit fit(2, n-2);

	uint i;
	for (i = 1; i < n-1; ++i) {
		real_type t = parameters[i];
		real_type u = 1-t;
		real_type r1 = 3*t*u*u;
		real_type r2 = 3*u*t*t;
		fit.setX(i - 1, 0, r1);		
		fit.setX(i - 1, 1, r2);		
	}
	fit.prepare();

	//fit x's
	for (i = 1; i < n-1; ++i) {
		real_type t = parameters[i];
		real_type u = 1-t;
		real_type r0 = u*u*u;
		real_type r3 = t*t*t;
		fit.sety(i - 1, points[i].x - r0 * points[0].x - r3 * points[n-1].x);
	}
	real_type chisq_x = fit.run();
	start_control().x = fit.c(0);
	finish_control().x = fit.c(1);

	//fit y's
	for (i = 1; i < n-1; ++i) {
		real_type t = parameters[i];
		real_type u = 1-t;
		real_type r0 = u*u*u;
		real_type r3 = t*t*t;
		fit.sety(i - 1, points[i].y - r0 * points[0].y - r3 * points[n-1].y);
	}
	real_type chisq_y = fit.run();
	start_control().y = fit.c(0);
	finish_control().y = fit.c(1);

	return chisq_x + chisq_y;
}*/

template float bezier<float>::best_fit_general(outline_us const & points, std::vector<float>::const_iterator);
template float bezier<float>::best_fit_general(outline_s const & points, std::vector<float>::const_iterator);
template float bezier<float>::best_fit_general(outline_i const & points, std::vector<float>::const_iterator);
template float bezier<float>::best_fit_general(outline_f const & points, std::vector<float>::const_iterator);
template double bezier<double>::best_fit_general(outline_d const & points, std::vector<double>::const_iterator);

template float bezier<float>::best_fit_general(outline_us const & points, uniformly_sampled_parameters<float>);
template float bezier<float>::best_fit_general(outline_s const & points, uniformly_sampled_parameters<float>);
template float bezier<float>::best_fit_general(outline_i const & points, uniformly_sampled_parameters<float>);
template float bezier<float>::best_fit_general(outline_f const & points, uniformly_sampled_parameters<float>);
template double bezier<double>::best_fit_general(outline_d const & points, uniformly_sampled_parameters<double>);

} //namespace outline
